#ifndef __MSG_CONVERT_H__
#define __MSG_CONVERT_H__

#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h> 
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/Twist.h>

#include <mc_core/common/point.h>


namespace msg_convert{

    /***************************************************
     * to string
     ****************************************************/
    // geometry_msgs::Pose -> string
    string toString(geometry_msgs::Pose pose);
    // geometry_msgs::Twist -> string
    string toString(geometry_msgs::Twist vel);

    /***************************************************
     * to mc::Point2f
     ****************************************************/
    mc::Point2f toPoint2f(geometry_msgs::Pose pose);

    /***************************************************
     * to Pose
     ****************************************************/
    

    /***************************************************
     * to PoseStamped
     ****************************************************/
    geometry_msgs::PoseStamped toPoseStamped(geometry_msgs::Pose pose, string frame_id);

    /***************************************************
     * to Path
     ****************************************************/
    nav_msgs::Path toPath(vector<mc::Point2f>& points, string frame_id);


    /***************************************************
     * Quaternion 转换
     ****************************************************/
    // geometry_msgs::Quaternion -> tf2::Quaternion
    tf2::Quaternion toTFQuaternion(geometry_msgs::Quaternion q);
    // tf2::Quaternion -> geometry_msgs::Quaternion
    geometry_msgs::Quaternion toGeometryQuaternion(tf2::Quaternion q);

    /***************************************************
     * 四元数 与 欧拉角、偏航角 转换
     ****************************************************/
    //euler -> tf2::Quaternion
    tf2::Quaternion toTFQuaternion(double roll, double pitch, double yaw);
    // euler -> geometry_msgs::Quaternion
    geometry_msgs::Quaternion toGeometryQuaternion(double roll, double pitch, double yaw);
    // tf2::Quaternion -> euler
    void toEuler(tf2::Quaternion q_tf, double& roll, double& pitch, double& yaw);
    // geometry_msgs::Quaternion -> euler
    void toEuler(geometry_msgs::Quaternion q, double& roll, double& pitch, double& yaw);
    // tf2::Quaternion -> yaw
    double toYaw(tf2::Quaternion q_tf);
    // geometry_msgs::Quaternion -> yaw
    double toYaw(geometry_msgs::Quaternion q);

    

};  // namespace

#endif